#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__
#include <math.h>
#include "main.h"
// 数学常量
#ifndef PI
#define PI 3.14159265358979323846
#endif


// 机械臂连杆长度结构体（单位：mm）
typedef struct {
    double l1;  // 基座到肩部
    double l2; // 肩部到肘部
    double l3;  // 肘部到腕部
    double l4;  // 腕部到末端1
    double l5;  // 末端1到末端2
} ArmDimensions;

// 关节角度结构体（单位：弧度）
typedef struct {
    double theta1;  // 基座旋转
    double theta2;  // 肩部俯仰
    double theta3;  // 肘部俯仰
    double theta4;  // 腕部俯仰
    double theta5;  // 末端旋转

} JointAngles;

#pragma pack(push, 1)
typedef struct {
    uint8_t header;  // 0xA5
    double grip_x;      // 8字节
    double grip_y;
    double grip_z;
    double release_x;
    double release_y;
    double release_z;
} PoseData;
#pragma pack(pop)

#pragma pack(push, 1)
typedef struct {
    uint8_t header;  // 0xA6
    double axis_1;   // 8字节
    double axis_2;
    double axis_3;
    double axis_4;
    double axis_5;
    double axis_6;
} AnglesData;
#pragma pack(pop)


// 末端位姿结构体
typedef struct {
    double x, y, z;  // 位置（mm）
    double pitch;    // 俯仰角ψ（弧度）平行为0度
    double roll;     // 翻滚角φ（弧度）左旋是0度，右旋是180度
} EndEffectorPose;

typedef enum {
    wait,
    grip,
    release,
    idle,
}KinematicsState;

void rad_to_deg(JointAngles* angles);
int inverseKinematics(const ArmDimensions* dimensions, EndEffectorPose* pose, JointAngles* angles);

#endif // __KINEMATICS_H__
